#define F_CPU 20000000UL  // 20 MHz CPU clock

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "serial.h"

// Global variables
Serialt serial;
uint8_t charIn; 				//USART incoming character
char in_str[5];
char str[30];
int current_pos, target_pos;
int mode = 0;
int tick=0;

void set_pwm(int pwm)
{
	OCR1BH = pwm >> 8;
	OCR1BL = pwm & 0xFF;

	return;
}

ISR (TIMER1_COMPB_vect) // Timer 1 is a 16-bit timer
{
	//This timer is for the pwm
	tick=tick+1;
}

void commMonitor(void)
{
	// Deal with incoming serial bytes here
	if (serial_read(&serial,&charIn)) //if there's data read it
	{
		serial_write(&serial, charIn); //echo it back out for now...
		if(mode==0)
		{
			switch (charIn)
			{
				case 'h':
				//	"Hello World"
					sprintf(str,"\rHello world!");
					for(int i=0; i<13; i++)
						serial_write(&serial, str[i]);
					break;
				case 'n':
				//	New target value
					sprintf(str,"\rNew Target:");
					for(int i=0; i<12; i++)
						serial_write(&serial, str[i]);
					mode = 4;
					break;
				case 'l':
				//	"Look" at current value
					sprintf(str, "\rCurrent(%d)/Target(%d)", current_pos, target_pos);
					for(int i=0; i<28; i++)
						serial_write(&serial, str[i]);
					break;
				case 'g':
				//	"Go" to set value
					sprintf(str,"\rMoving to: %d", target_pos);
					for(int i=0; i<16; i++)
						serial_write(&serial, str[i]);
					set_pwm(target_pos);
					current_pos = target_pos;
					break;
				default:
					//	Throw error?
					break;
			}
		}
		else if(mode == 4)
		{
			in_str[0] = charIn;
			mode--;
		}
		else if(mode == 3)
		{
			in_str[1] = charIn;
			mode--;
		}
		else if(mode == 2)
		{
			in_str[2] = charIn;
			mode--;
		}
		else if(mode == 1)
		{
			in_str[3] = charIn;
			in_str[4] = '\0';
			target_pos = atoi(in_str);
			mode--;
		}
		else
		{
			// Error LED on
			PORTC |= _BV(PC1);
		}
	}

	return;
}


int main(void) 
{
	
	// Initialization
	
	// 	Set up serial port
	serial_init(&serial);
	
	//	Set up IO pin
	DDRC |= _BV(PC1); 	// set led pin to output direction
	PORTC &= ~_BV(PC1); 	// led on, pin high
	DDRB |= _BV(PB2);	// enable pwm output pin
	
	//	Set up pwm timer
	TCCR1A |= _BV(5); //WGM's cleared for PWM phase and freq correct mode, non-inverting output, 
								//Compare Match B
	TCCR1B |= _BV(4) | _BV(1); //Clock prescaler 8
	ICR1H = 0x61; //0x61A8 is 25000 as TOP for 50 Hz
	ICR1L = 0xA8;
	TIMSK1 |= _BV(2); // Enable timer interrupt OCIE1B

	//	Set pwm value here
	current_pos=target_pos=1250;
	set_pwm( current_pos ); 

	// Start timers
	TCNT1H = 0; // Set timer high bits
	TCNT1L = 0; // Set timer low bits
	TCNT0 = 0;

	sei(); // Globally enable interrupts
         
    while (1)                       /* loop forever */
    {
		commMonitor();	// Monitor serial port for incoming bytes
		
		//	Interrupt triggers at 81 Hz;
		/*if(tick >= 100)
		{
			sprintf(str,"\rPWM: %d", current_pos);
			for(int i=0; i<12; i++)
				serial_write(&serial, str[i]);
			tick = 0;
		}*/
    }
	
	
	return 0;
}
